﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;


namespace PPP
{

    struct vector
         {
             private double x;

             public double X
             {
                 get { return x; }
                 set { x = value; }
             }
             private double y;

             public double Y
             {
                 get { return y; }
                 set { y = value; }
             }
             private double z;

             public double Z
             {
                 get { return z; }
                 set { z = value; }
             }

             private double mag;

             public double Mag
             {
                 get { return mag; }
                 set { mag = value; }
             }
  



         }
        
    class MapControl : Panel
    {

        public delegate void FunctionPointer(List<FlyingCommands> listComands);
        public event FunctionPointer InfoChage;


       public List<waypoint> ListWaypoits = new List<waypoint>();
       public int thisisparent=0;      
       bool addWaypoint = true;
       bool removeWaypoint = false;
       public int RealMapWidth=0;
       public int RealMapHeight = 0; 
       private float UAVAngleFromOutside = 0;
       public UAVIcon uav = new UAVIcon();
       public float UAVAngleFromOutside1
       {
           get { return UAVAngleFromOutside; }
           set { UAVAngleFromOutside = value; }
       }
       private bool addUAV = false;
       private int UavNextWaypoint = 0;
       private List<FlyingCommands> listComands;

       internal List<FlyingCommands> ListComands
       {
           get { return listComands; }
           set { listComands = value; }
       }


       public MapControl()
        {

            // Double bufferisation
            SetStyle(ControlStyles.DoubleBuffer | ControlStyles.UserPaint | ControlStyles.AllPaintingInWmPaint, true);
            this.BackColor = Color.Aqua;
            this.Click += click;

            this.ControlRemoved += new ControlEventHandler(MapControl_ControlRemoved);

            this.ControlAdded += new ControlEventHandler(MapControl_ControlAdded);
           
            
        }

        public void ImoveInsideMap(){

            //Rase Event

            if (InfoChage != null)
            {
                this.InfoChage(this.GetComandsToSend());
            }

        }
        

       void MapControl_ControlAdded(object sender, ControlEventArgs e)
       {
           //Rase Event

           if (InfoChage != null)
           {
               this.InfoChage(this.GetComandsToSend());
           }
       }

       void MapControl_ControlRemoved(object sender, ControlEventArgs e)
       {

           //Rase Event

           if (InfoChage != null)
           {
               this.InfoChage(this.GetComandsToSend());
           }



       }
        
        public bool AddWaypoint
        {
            get { return addWaypoint; }
            set { addWaypoint = value; }
        }
             
        public bool RemoveWaypoint
        {
            get { return removeWaypoint; }
            set { removeWaypoint = value; }
        }
               
        protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
        {
            base.OnPaint(e);

            List<Point> listofPoints = new List<Point>();
            Point dummyP = new Point();



            if (ListWaypoits.Count >= 2)
            {
                foreach (waypoint item in ListWaypoits)
                {

                    dummyP = item.Location ;

                    dummyP.X = dummyP.X + item.Width / 2;
                    dummyP.Y = dummyP.Y + item.Height / 2;

                    listofPoints.Add(dummyP);
                }

                
                e.Graphics.DrawLines(new Pen(Color.Black, 3), listofPoints.ToArray());

                if (Contains(uav))
                {

                    try
                    {
                        int wpx = ListWaypoits.ElementAt(UavNextWaypoint).Location.X + ListWaypoits.ElementAt(UavNextWaypoint).Width / 2;
                        int wpy = ListWaypoits.ElementAt(UavNextWaypoint).Location.Y + ListWaypoits.ElementAt(UavNextWaypoint).Height / 2;

                        int uavx = uav.Location.X;

                        int uavy = uav.Location.Y;



                        e.Graphics.DrawLine(new Pen(Color.Wheat, 2), wpx, wpy, uavx + uav.Width / 2, uavy + uav.Height / 2);
                    }
                    catch (Exception ex)
                    {
                       
                    }



                }





            }


            if(Contains(uav)){

                int UAVarrowLength =  this.Width / 10;





                int x2 = (int)((uav.Left + uav.Width / 2) + UAVarrowLength * Math.Cos(uav.WaypointT * (Math.PI / 180)));
              int y2 = (int)( UAVarrowLength * Math.Sin(uav.WaypointT * (Math.PI / 180)));






              e.Graphics.DrawLine(new Pen(Color.Wheat, 2), uav.Left + uav.Width / 2, uav.Top + uav.Height / 2, x2,  (uav.Top + uav.Height / 2)- y2);


            }







        }

        public void click(object sender, EventArgs e)
        {

            if (addWaypoint == true && RealMapWidth >= 1 && this.Contains(uav))
            {

                // MessageBox.Show(" "+thisisparent);
                waypoint p = new waypoint();
                p.Location = new Point(((MouseEventArgs)e).Location.X, ((MouseEventArgs)e).Location.Y);
                p.TextAlign = ContentAlignment.MiddleCenter;


                //From Pixel Coordinates to Meters
                p.WaypointX = (float)((float)RealMapWidth * (float)p.Location.X) / (float)this.Width;
                p.WaypointY = (float)((float)RealMapHeight * ((float)this.Height-(float)p.Location.Y)) / (float)this.Height;
                p.WaypointZ = 1;
                
                


                ListWaypoits.Add(p);
                p.Text = "" + ListWaypoits.IndexOf(p);
                p.Name = "" + ListWaypoits.IndexOf(p);
                
                Controls.Add(p);


                Refresh();





            }
            
            if(RealMapWidth < 1)
            {

                MessageBox.Show("Please provide width of Map in Meters");


            }

        }
        
        public void clearMap(){


            if (ListWaypoits.Count != 0)
            {



                foreach (waypoint item in ListWaypoits)
                {
                    Controls.Remove(item);
                    
                }


                ListWaypoits.Clear();

            }
            ImoveInsideMap();
            Refresh();

        }
        
        public void SetUAVDefaultLocOnMap()
        {
            
            if (addUAV == false)
            {

                //add UAV Icon
                Controls.Add(uav);
                addUAV = true;

            }

            if(Controls.Contains(uav)){
            //Adds UAV to bottom left Representing 0,0
                //this is in the map in pixels
            uav.Left = -uav.Width / 2;
            uav.Top = this.Height - uav.Height / 2;
                //This is in real world in Meters
            uav.WaypointX = 0;
            uav.WaypointY = 0;
            uav.WaypointZ = 0;
            uav.WaypointT = 0;
            //to raise Evant inside Map or Let the Map Raise the Event for him
            ImoveInsideMap();
            Refresh();
            }



        }

        public void UpdateUAVonMap(float WaypointX, float WaypointY, float WaypointZ, float WaypointT, int uavNextWaypoint)
        {
            if (Controls.Contains(uav) && RealMapWidth >= 1)
            {

                Point p = new Point((int)((this.Width * WaypointX) / this.RealMapWidth), (int)(this.Height - ((this.Height * WaypointY) / this.RealMapHeight)));


                uav.Location = p;



                uav.WaypointX = WaypointX;
                uav.WaypointY = WaypointY;
                uav.WaypointZ = WaypointZ;
                uav.WaypointT = WaypointT;
                UavNextWaypoint = uavNextWaypoint;

                //to raise Event inside Map or Let the Map Raise the Event for him
                ImoveInsideMap();
                Refresh();
            }

        }

        public void UpdatePixelToMeters()
        {
            if (ListWaypoits.Count != 0)
            {

                uav.UAVpixelToMeters();


                foreach (waypoint item in ListWaypoits)
                {

                    //From Pixel Coordinates to Meters
                    item.WaypointX = (float)((float)RealMapWidth * (float)item.Location.X) / (float)this.Width;
                    item.WaypointY = (float)((float)RealMapHeight * ((float)this.Height - (float)item.Location.Y)) / (float)this.Height;
                    item.WaypointZ = 1;


                }

                //to raise Evant inside Map or Let the Map Raise the Event for him
                ImoveInsideMap();
                Refresh();

            }

        }

      

        public void addWaypointF(float wx,float wy,float wz)
        {
            if (addWaypoint == true && RealMapWidth >= 1)
            {
                if (wx >= 0 && wy >= 0 && wz >= 0 && RealMapWidth >= wx && RealMapWidth >= wy && RealMapWidth >= wz)
                {
                    
                    // MessageBox.Show(" "+thisisparent);
                    waypoint p = new waypoint();
                    p.Location = new Point((int)((this.Width * wx) / this.RealMapWidth), (int)(this.Height - ((this.Height * wy) / this.RealMapHeight)));
                    p.TextAlign = ContentAlignment.MiddleCenter;
                    
                    p.WaypointX = wx;
                    p.WaypointY = wy;
                    p.WaypointZ = wz;

                    ListWaypoits.Add(p);
                    p.Text = "" + ListWaypoits.IndexOf(p);
                    p.Name = "" + ListWaypoits.IndexOf(p);

                    Controls.Add(p);

                    Refresh();
                }
            }

            if (RealMapWidth < 1)
            {
                MessageBox.Show("Please provide width of Map in Meters");
            }

        }

        private vector CrossPruduct(vector a, vector b)
        {

            vector c = new vector();

            c.X = +a.Y * b.Z - a.Z * b.Y;
            c.Y = +a.Z * b.X - a.X * b.Z;
            c.Z = +a.X * b.Y - a.Y * b.X;

            return c;
           
        }
        
        private double DotProduct(vector a, vector b)
        {

            //This is to make dot product 2d only
            double tempa = a.Z;
            double tempb = b.Z;
            a.Z = 0;
            b.Z = 0;
            /////////////////////////

            double aDotb = a.X * b.X + a.Y * b.Y + a.Z * b.Z;

            double maga = Math.Sqrt((a.X * a.X) + (a.Y * a.Y) + (a.Z * a.Z));
            double magb = Math.Sqrt((b.X * b.X) + (b.Y * b.Y) + (b.Z * b.Z));

            double theta = (Math.Acos((aDotb) / (maga * magb)) * 180) / Math.PI;
          
            
            //This is to make dot product 2d only
            a.Z=tempa;
            b.Z = tempb;
            /////////////////////////


            return theta;

        }
        
        public List<FlyingCommands> GetComandsToSend()
        {       

            try
            {
                List<vector> ListVectors = new List<vector>();

///////////////////////////////UAV Vector/////////////////////////////////
                double varX = Math.Cos((Math.PI / 180) * uav.WaypointT) ;
                double varY = Math.Sin((Math.PI / 180) * uav.WaypointT) ;
                vector v = new vector();
                v.X = varX;
                v.Y = varY;
                v.Z = 0;
                v.Mag = Math.Sqrt((varX * varX) + (varY * varY));

                ListVectors.Add(v);

                double lastpositionX = uav.WaypointX;
                double lastpositionY =uav.WaypointY;

//////////////////////////All waypoints////////////////////////////////////
                foreach (waypoint wayP in ListWaypoits)
                {
                   varX = wayP.WaypointX - lastpositionX;
                   varY = wayP.WaypointY - lastpositionY;

                   v = new vector();
                   v.X = varX;
                   v.Y = varY;
                   v.Z = wayP.WaypointZ;
                   v.Mag = Math.Sqrt((varX * varX) + (varY * varY));
                   ListVectors.Add(v);

                   lastpositionX = wayP.WaypointX;
                   lastpositionY = wayP.WaypointY;

                }
/////////////////////////To Array/////////////////////////////////////////

                vector[] VectorArray = ListVectors.ToArray();

                listComands = new List<FlyingCommands>();
                FlyingCommands FC;


                for (int i = 1; i <= ListVectors.Count-1; i++)
                {
                    v = CrossPruduct(VectorArray[i - 1], VectorArray[i]);

                    if (v.Z > 0)
                    {

                        FC = new FlyingCommands();
                        FC.Angle =Math.Round(DotProduct(VectorArray[i - 1], VectorArray[i]) * -1,0);
                        FC.Distance = Math.Round(VectorArray[i].Mag,0);
                        FC.Altitude = Math.Round(VectorArray[i].Z, 0);
                        FC.Name = (i - 1).ToString();
                        listComands.Add(FC);

                        
                    }
                    else if (v.Z < 0)
                    {
                        FC = new FlyingCommands();
                        FC.Angle = Math.Round(DotProduct(VectorArray[i - 1], VectorArray[i]) * 1,0);
                        FC.Distance = Math.Round(VectorArray[i].Mag,0);
                        FC.Altitude = Math.Round(VectorArray[i].Z,0);
                        FC.Name = (i - 1).ToString();
                        listComands.Add(FC);


                    }
                    else
                    {
                        FC = new FlyingCommands();
                        FC.Angle = Math.Round(DotProduct(VectorArray[i - 1], VectorArray[i]) * 1,0);
                        FC.Distance = Math.Round(VectorArray[i].Mag,0);
                        FC.Altitude = Math.Round(VectorArray[i].Z, 0);
                        FC.Name = (i - 1).ToString();
                        listComands.Add(FC);


                    }

                }








           // MessageBox.Show("Num of FlyinDisAgl: " + lda.Count + "Magnitude to this Waypoint: " + RealComands.ElementAt(0).Distance + " Dir from Y axis : " + RealComands.ElementAt(1).Angle1);
           // MessageBox.Show("Num of FlyinDisAgl: " + lda.Count + "Magnitude to this Waypoint: " + RealComands.ElementAt(1).Distance + " Dir from Y axis : " + RealComands.ElementAt(2).Angle1);


          // MessageBox.Show("Num of FlyinDisAgl: " + lda.Count + "Magnitude to this Waypoint: " + lda.ElementAt(3).Distance + " Dir from X axis : " + lda.ElementAt(3).Angle1);
            //MessageBox.Show("Num of FlyinDisAgl: " + lda.Count + "Magnitude to this Waypoint: " + lda.ElementAt(3).Distance + " Dir from X axis : " + lda.ElementAt(3).Angle1);





                return listComands;


            }
            catch (Exception Ex)
            {

                return null;
            }










        }
        
    }
    
    class waypoint : Label
    {


        private string waypointName = "";
        private float waypointX = 0; 
        private float waypointY = 0;
        private float waypointZ = 0;
        

        public string WaypointName
        {
        get { return waypointName; }
        set { waypointName = value; }
        }

        public float WaypointX
        {
        get { return waypointX; }
        set { waypointX = value; }
        }

        public float WaypointY
        {
        get { return waypointY; }
        set { waypointY = value; }
        }

        public float WaypointZ
        {
        get { return waypointZ; }
        set { waypointZ = value; }
        }

       


        public waypoint()
        {
            this.BackColor = Color.White;
            this.Size = new Size(20, 20);            
            this.MouseClick += click;
            this.MouseDown += mouseDown;
            this.MouseMove += mouseMove;


            this.Cursor = Cursors.Default;
           

        }


        protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
        {
            base.OnPaint(e);
                   



        }

        int x = 0;
        int y = 0;



        public void mouseDown(object sender, EventArgs e)
        {
            if (((MouseEventArgs)e).Button == MouseButtons.Left)
            {
                x = ((MouseEventArgs)e).X;
                y = ((MouseEventArgs)e).Y;
            } 

        }

        public void mouseMove(object sender, EventArgs e)
        {

            if (((MouseEventArgs)e).Button == MouseButtons.Left)
            {
                this.Left += (((MouseEventArgs)e).X - x);
                this.Top += (((MouseEventArgs)e).Y - y);
            }

            ((MapControl)this.Parent).Refresh();
        }
        
        public void click(object sender, EventArgs e)
        {



            if (((MouseEventArgs)e).Button == MouseButtons.Middle)
            {
               




            }


            if (((MouseEventArgs)e).Button == MouseButtons.Left)
            {
                ///////////////////////////////////////////////////////Updates the waypoint position when is moved on the map//////////////////////////////////////////////////////////////////////////////////
                // ((MapControl)this.Parent)       //this is the parent;
                //   ((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name))  //This is the waypoint

                //From Pixel Coordinates to Meters
                ((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).WaypointX = (float)((float)((MapControl)this.Parent).RealMapWidth * (float)((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).Location.X) / (float)((MapControl)this.Parent).Width;
                ((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).WaypointY = (float)((float)((MapControl)this.Parent).RealMapHeight * ((float)((MapControl)this.Parent).Height - (float)((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).Location.Y)) / (float)((MapControl)this.Parent).Height;
                ((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).WaypointZ = waypointZ;

                //to raise Evant inside Map or Let the Map Raise the Event for him
                ((MapControl)this.Parent).ImoveInsideMap();

                // Debugging 
                //   MessageBox.Show("Name: "+((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).Name + " X = " + ((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).WaypointX + " m " + "Y = " + ((MapControl)this.Parent).ListWaypoits.ElementAt(int.Parse(this.Name)).WaypointY + " m ");
                /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////  

                ///////////////////////////////Remove this specific waypoint////////////////////////////////////////
                if (((MapControl)this.Parent).RemoveWaypoint == true)
                {
                    //MessageBox.Show(" " + ((MapControl)this.Parent).ListWaypoits.IndexOf(this));
                    ((MapControl)this.Parent).ListWaypoits.Remove(this);

                    foreach (waypoint item in ((MapControl)this.Parent).ListWaypoits)
                    {
                        item.Text = "" + ((MapControl)this.Parent).ListWaypoits.IndexOf(item);
                        item.Name = "" + ((MapControl)this.Parent).ListWaypoits.IndexOf(item);
                    }

                    ((MapControl)this.Parent).Refresh();

                    this.Dispose();
                }
                ////////////////////////////////////////////////////////////////////////////////////////////////////
            }

           


        }
    }
    
    class UAVIcon : Label
    {
        private float waypointX = 0;
        private float waypointY = 0;
        private float waypointZ = 0;
        private float waypointT = 0;

        public float WaypointT
        {
            get { return waypointT; }
            set { waypointT = value; }
        }
        

      private  int x = 0;
      private  int y = 0;

        public float WaypointX
        {
            get { return waypointX; }
            set { waypointX = value; }
        }

        public float WaypointY
        {
            get { return waypointY; }
            set { waypointY = value; }
        }

        public float WaypointZ
        {
            get { return waypointZ; }
            set { waypointZ = value; }
        }

        public UAVIcon()
        {


            this.BackColor = Color.Gold;
            this.Size = new Size(20, 20);
          
            this.MouseDown += mouseDown;
            this.MouseMove += mouseMove;
            this.MouseClick += mouseClick;

            this.Cursor = Cursors.Default;

        }

        public void mouseClick(object sender, EventArgs e)
        {
            UAVpixelToMeters();


            //to raise Evant inside Map or Let the Map Raise the Event for him
            ((MapControl)this.Parent).ImoveInsideMap();
           ((MapControl)this.Parent).Refresh();

        }


        public void UAVpixelToMeters()
        {


            //From Pixel Coordinates to Meters
            ((MapControl)this.Parent).uav.WaypointX = (float)((float)((MapControl)this.Parent).RealMapWidth * (float)((MapControl)this.Parent).uav.Location.X) / (float)((MapControl)this.Parent).Width;
            ((MapControl)this.Parent).uav.WaypointY = (float)((float)((MapControl)this.Parent).RealMapHeight * ((float)((MapControl)this.Parent).Height - (float)((MapControl)this.Parent).uav.Location.Y)) / (float)((MapControl)this.Parent).Height;
            ((MapControl)this.Parent).uav.WaypointZ = ((MapControl)this.Parent).uav.WaypointZ;

        }



        public void mouseDown(object sender, EventArgs e)
        {
            if (((MouseEventArgs)e).Button == MouseButtons.Left)
            {
                x = ((MouseEventArgs)e).X;
                y = ((MouseEventArgs)e).Y;
            }

        }

        public void mouseMove(object sender, EventArgs e)
        {

            if (((MouseEventArgs)e).Button == MouseButtons.Left)
            {
                this.Left += (((MouseEventArgs)e).X - x);
                this.Top += (((MouseEventArgs)e).Y - y);
            }

            ((MapControl)this.Parent).Refresh();
        }



    }


class FlyingCommands
        {
        private double distance;
        private double angle;
        private double altitude;
        private string name = "";

        public string Name
        {
            get { return name; }
            set { name = value; }
        }

            public double Distance
        {
        get { return distance; }
        set { distance = value; }
        }
            public double Angle
        {
            get { return angle; }
            set { angle = value; }
        }
            public double Altitude
        {
            get { return altitude; }
            set { altitude = value; }
        }


          public  FlyingCommands()
            {


            }
       
        
        
        }

















}
